%% cost function
function ret = cost(aflat)
global ns_time s_time ns_slope s_knee ns_knee hip_pos pos_com
% global kneemin hmin hmax qdotmax umax deltaP powermax kneemax

%%%%%%%%%%%%%% Unflatten the a matrix
a = unflatten(aflat);

%%%%%Pick the cost function
%%% 0 = torque based cost, requires additional integration
%%% 1 = human data based cost
typeofcost = 1;

%%%%%The torque based cost
if typeofcost == 0
    
    ret = torque_cost(a);
    
    %%%% The data based cost
elseif typeofcost == 1
    
    ret = humandata_cost(a);
end

end

function ret = torque_cost(a)
%%% Determine the point on the guard through inverse kinematics
[x_minus,Ttau,yminus,H,phipcond] = hzd_con(a);
%%% Integrate the system forward one step
x_plus = resetFunc(x_minus);
[Tf,xf] = step_under_reduced(x_plus,a,Ttau);

%%% Determine the step length (if needed
posf = jpos_mat(xf);
steplength = posf(1,end);
if steplength < 0
    steplength = 0.0001;
end

%%% Compute the cost
% ret =  norm(xf-x_minus)^2;
% ret = umax/abs(steplength)+norm(xf-x_minus)^2;
% ret = umax/abs(steplength);
%  ret = umax/steplength;
ret = (umax)/steplength;
end

function ret = humandata_cost(a)
global ns_time s_time ns_slope s_knee ns_knee hip_pos pos_com
%%%% Decide if you want to scale time based upon the parameterization of
%%%% time
scaletime = 0;
if scaletime == 0
    
    time_scale = ns_time;
    
elseif scaletime == 1
    
    [x_minus,Ttau,yminus,H,phipcond] = hzd_con(a);
    time_scale = ns_time*(Ttau/ns_time(end));
    
end

%%%%%%%%%%%%%% Cost for each canonical function

cost_ns_slope = sum(abs(ns_slope-yd_ns_slope(time_scale,a)));
cost_hip_pos = sum(abs(hip_pos-yd_hip_pos(time_scale,a)));
cost_s_knee = sum(abs(s_knee-yd_s_knee(time_scale,a)));
cost_ns_knee = sum(abs(ns_knee-yd_ns_knee(time_scale,a)));
cost_pos_com = sum(abs(pos_com-yd_pos_com(time_scale,a)));

%%%%%%%%%%%%%% Wieghting for the fits (in this case the torso cost is
%%%%%%%%%%%%%% not used.

beta_ns_slope = 1/(max(ns_slope)-min(ns_slope));
beta_hip_pos = 1/(max(hip_pos)-min(hip_pos));
beta_s_knee = 1/(max(s_knee)-min(s_knee));
beta_ns_knee = 1/(max(ns_knee)-min(ns_knee));
beta = beta_ns_slope+beta_hip_pos+beta_s_knee+beta_ns_knee;
% %         beta_pos_com = 1; %/(max(pos_com)-min(pos_com));
beta_pos_com = 1/(max(pos_com)-min(pos_com));
beta = beta_ns_slope+beta_hip_pos+beta_s_knee+beta_ns_knee+beta_pos_com;

%%% Determine the cost
cost_all = [
    beta_ns_slope*cost_ns_slope,...
    beta_hip_pos*cost_hip_pos,...
    beta_s_knee*cost_s_knee,...
    beta_ns_knee*cost_ns_knee,...
    beta_pos_com*cost_pos_com
    ];

%%%%%%%%%%%%%% Return the cost
ret = sum(cost_all)/beta;
end
